package com.lyfam.game.behaviosr.steering;

import java.util.ArrayList;
import java.util.List;

import processing.core.PGraphics;

import com.lyfam.game.entity.BaseGameEntity;
import com.lyfam.game.entity.MovingEntity;
import com.lyfam.game.entity.Obstacle;
import com.lyfam.game.geometry.CommonGeometry;
import com.lyfam.game.geometry.Vector2D;

public class ObstacleAvoidanceBehavior implements ISteeringBehavior
{
	private MovingEntity veh;
	private List<Obstacle> obstacles;
	private PGraphics g;

	public ObstacleAvoidanceBehavior(MovingEntity veh, List<Obstacle> obs,
			PGraphics g)
	{
		this.veh = veh;
		this.obstacles = obs;
		this.g = g;
	}

	@Override
	public int getPriority()
	{
		return 0;
	}

	private Vector2D calculateSteeringForce()
	{
		List<Obstacle> obstaclesInRange = this.getObstaclesWithinRange();
		BaseGameEntity closestObstacle = null;
		float distanceToClosestIP = Float.MAX_VALUE;

		Vector2D localPosOfClosestObstacle = null;

		float xPos = veh.getPosition().getX();
		float yPos = veh.getPosition().getY();
		float theta = veh.getVelocity().heading2D();
		g.pushMatrix();
		g.translate(xPos, yPos);
		g.rotate(theta);
		
		g.rect(-veh.getSize(), veh.getBoundary() / 2, this.getDetectionBoxLength(), -veh.getBoundary());
		
		for (Obstacle obstacle : obstaclesInRange)
		{
			// convert the obstacle to the vehicle local coordinate system
			Vector2D localPos = CommonGeometry.ConvertFromWorldToLocal(veh,
					obstacle.getPosition());

			//g.ellipse(localPos.getX(), localPos.getY(), obstacle.getSize() * 2, obstacle.getSize() * 2);
			if (obstacle.getPosition().getX() > 0)
			{
				float expandedRadius = obstacle.getBoundary()
						+ this.veh.getBoundary();
				
				if (Math.abs(localPos.getY()) < expandedRadius)
				{
					float cX = localPos.getX();
					float cY = localPos.getY();
					float sqrtPart = (float) Math.sqrt(expandedRadius
							* expandedRadius - cY * cY);
					float ip = cX - sqrtPart;
					if (ip <= 0)
					{
						ip = cX + sqrtPart;
					}

					if (ip < distanceToClosestIP)
					{
						distanceToClosestIP = ip;
						closestObstacle = obstacle;
						localPosOfClosestObstacle = localPos;
					}
				}
			}
		}

		// calculating the stering force
		Vector2D steeringForce = new Vector2D();
		if (closestObstacle != null)
		{
			float detectionBLength = this.getDetectionBoxLength();
			
			float multiplier = 1.0f
					+ (detectionBLength - localPosOfClosestObstacle.getX())
					/ detectionBLength;

			steeringForce
					.setY((closestObstacle.getBoundary() - localPosOfClosestObstacle
							.getY())
							* multiplier);

			float brakingWeight = 0.2f;

			steeringForce
					.setX((closestObstacle.getBoundary() - localPosOfClosestObstacle
							.getX())
							* brakingWeight);
			
		}
		
		g.line(0, 0, steeringForce.getX(), steeringForce.getY());
		
		float worldX = g.screenX(steeringForce.getX(), steeringForce.getY());
		float worldY = g.screenY(steeringForce.getX(), steeringForce.getY());
		steeringForce.setX(worldX);
		steeringForce.setY(worldY);
		
		g.popMatrix();

		return steeringForce;
	}

	@Override
	public Vector2D getSteeringForce()
	{
		return this.calculateSteeringForce();
	}

	private List<Obstacle> getObstaclesWithinRange()
	{
		List<Obstacle> theList = new ArrayList<Obstacle>(this.obstacles.size());
		float dBoxLength = this.getDetectionBoxLength();

		for (Obstacle obstacle : this.obstacles)
		{
			Vector2D to = Vector2D.clone(obstacle.getPosition());
			to.sub(this.veh.getPosition());
			float range = dBoxLength + obstacle.getBoundary();

			if (to.getLengthSquare() < range * range)
			{
				theList.add(obstacle);
			}
		}

		return theList;
	}

	private float getDetectionBoxLength()
	{
		float minDBoxLength = this.veh.getSize() * 3;
		return minDBoxLength
				+ (this.veh.getVelocity().getLength() / this.veh.getMaxSpeed())
				* minDBoxLength;
	}

	@Override
	public int compareTo(ISteeringBehavior o)
	{
		if (getPriority() == o.getPriority())
		{
			return 0;
		} else
		{
			if (getPriority() > o.getPriority())
			{
				return 1;
			} else
			{
				return -1;
			}
		}
	}

}
